﻿using System;
using System.Collections.Generic;
using BulletSharp;
using BulletSharp.Math;


namespace Painter3D.PhysicalSys
{
    class BulletEngine : IDisposable
    {
        private CollisionConfiguration configuration;
        private CollisionDispatcher dispatcher;
        private BroadphaseInterface broadphase;
        public static DiscreteDynamicsWorld world;

        public BulletEngine()
        {
            configuration = new DefaultCollisionConfiguration();
            dispatcher = new CollisionDispatcher(configuration);
            broadphase = new DbvtBroadphase();
            world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, configuration);
            Vector3 gravity = new Vector3(0, -9.8f, 0);
            world.SetGravity(ref gravity);
            world.DebugDrawer = new BulletDebugDrawer();
            world.DebugDrawer.DebugMode = DebugDrawModes.DrawWireframe | DebugDrawModes.DrawAabb;
            //FilterCallBack filterCallBack = new FilterCallBack();
            //world.PairCache.SetOverlapFilterCallback(filterCallBack);

        }

        public struct RayCastResult
        {
            /// <summary>
            /// 是否检测成功
            /// </summary>
            readonly public bool success;
            /// <summary>
            /// 被检测到的物体对应的节点
            /// </summary>
            readonly public Transform transform;
            /// <summary>
            /// 接触点
            /// </summary>
            readonly public Vector3 point;
            /// <summary>
            /// 接触点的法向量
            /// </summary>
            readonly public Vector3 normal;

            public RayCastResult(bool success, Transform transform, Vector3 point, Vector3 normal)
            {
                this.success = success;
                this.transform = transform;
                this.point = point;
                this.normal = normal;
            }
        }

        /// <summary>
        /// 进行一次射线检测,只检查最近的物体
        /// </summary>
        /// <param name="start">起点</param>
        /// <param name="end">终点</param>
        /// <returns>碰撞信息</returns>
        static public RayCastResult RayCastClosest(Vector3 start, Vector3 end)
        {
            //如果增加碰撞的层级支持，可能要修改ClosestRayResultCallback的创建
            using (var rayCallBack = new ClosestRayResultCallback(ref start, ref end))
            {
                //world.RayTest(start, end, rayCallBack);
                world.RayTestRef(ref start, ref end, rayCallBack);
                Vector3 position = default(Vector3);
                Vector3 normal = default(Vector3);
                MotionStateUpdate state = null;
                if (rayCallBack.HasHit)
                {
                    position = rayCallBack.HitPointWorld;
                    normal = rayCallBack.HitNormalWorld;
                    var body = rayCallBack.CollisionObject as BulletSharp.RigidBody;
                    state = body.MotionState as MotionStateUpdate;
                    return new RayCastResult(rayCallBack.HasHit, state.transform, position, normal);
                }
                return new RayCastResult(rayCallBack.HasHit, null, position, normal);
            }
        }

        /// <summary>
        /// 进行一次射线检测,返回所有的信息
        /// </summary>
        /// <param name="start">起点</param>
        /// <param name="end">终点</param>
        /// <returns>碰撞信息</returns>
        static public RayCastResult[] RayCastAll(Vector3 start, Vector3 end)
        {
            //如果增加碰撞的层级支持，可能要修改ClosestRayResultCallback的创建
            using (var rayCallBack = new AllHitsRayResultCallback(start, end))
            {
                world.RayTestRef(ref start, ref end, rayCallBack);
                if (!rayCallBack.HasHit)
                {
                    return null;
                }
                Vector3 position = default(Vector3);
                Vector3 normal = default(Vector3);
                MotionStateUpdate state = null;
                List<RayCastResult> result = new List<RayCastResult>();
                for (int i = 0; i < rayCallBack.CollisionObjects.Count; i++)
                {
                    position = rayCallBack.HitPointWorld[i];
                    normal = rayCallBack.HitNormalWorld[i];
                    var body = rayCallBack.CollisionObjects[i] as BulletSharp.RigidBody;
                    state = body.MotionState as MotionStateUpdate;
                    result.Add(new RayCastResult(true, state.transform, position, normal));
                }
                return result.ToArray();
            }
        }


        class FilterCallBack : OverlapFilterCallback
        {
            public override bool NeedBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1)
            {
                return true;
            }
        }

        public void Update()
        {
            world.StepSimulation(1 / 60.0f);
        }

        public void DebugDraw()
        {
            world.DebugDrawWorld();
        }

        #region 析构函数组
        public void Dispose()
        {
            CleanupConstraints(world);
            CleanupBodiesAndShapes(world);

            var multiBodyWorld = world as MultiBodyDynamicsWorld;
            if (multiBodyWorld != null)
            {
                CleanupMultiBodyWorld(multiBodyWorld);
            }

            world.Dispose();
            broadphase.Dispose();
            dispatcher.Dispose();
            configuration.Dispose();
        }

        private static void CleanupConstraints(DynamicsWorld world)
        {
            var nonWorldObjects = new HashSet<CollisionObject>();

            for (int i = world.NumConstraints - 1; i >= 0; i--)
            {
                TypedConstraint constraint = world.GetConstraint(i);
                world.RemoveConstraint(constraint);
                if (constraint.RigidBodyA.BroadphaseHandle == null)
                {
                    nonWorldObjects.Add(constraint.RigidBodyA);
                }
                if (constraint.RigidBodyB.BroadphaseHandle == null)
                {
                    nonWorldObjects.Add(constraint.RigidBodyB);
                }
                constraint.Dispose();
            }

            foreach (var obj in nonWorldObjects)
            {
                obj.Dispose();
            }
        }

        private static void CleanupBodiesAndShapes(DynamicsWorld world)
        {
            var shapes = new HashSet<CollisionShape>();

            for (int i = world.NumCollisionObjects - 1; i >= 0; i--)
            {
                CollisionObject obj = world.CollisionObjectArray[i];
                BulletSharp.RigidBody rigidBody = obj as BulletSharp.RigidBody;
                if (rigidBody != null && rigidBody.MotionState != null)
                {
                    rigidBody.MotionState.Dispose();
                }
                world.RemoveCollisionObject(obj);
                GetShapeWithChildShapes(obj.CollisionShape, shapes);

                obj.Dispose();
            }

            foreach (var shape in shapes)
            {
                shape.Dispose();
            }
        }

        private static void CleanupMultiBodyWorld(MultiBodyDynamicsWorld world)
        {
            for (int i = world.NumMultiBodyConstraints - 1; i >= 0; i--)
            {
                MultiBodyConstraint multiBodyConstraint = world.GetMultiBodyConstraint(i);
                world.RemoveMultiBodyConstraint(multiBodyConstraint);
                multiBodyConstraint.Dispose();
            }

            for (int i = world.NumMultibodies - 1; i >= 0; i--)
            {
                MultiBody multiBody = world.GetMultiBody(i);
                world.RemoveMultiBody(multiBody);
                multiBody.Dispose();
            }
        }

        private static void GetShapeWithChildShapes(CollisionShape shape, HashSet<CollisionShape> shapes)
        {
            shapes.Add(shape);

            var convex2DShape = shape as Convex2DShape;
            if (convex2DShape != null)
            {
                GetShapeWithChildShapes(convex2DShape.ChildShape, shapes);
                return;
            }

            var compoundShape = shape as CompoundShape;
            if (compoundShape != null)
            {
                foreach (var childShape in compoundShape.ChildList)
                {
                    GetShapeWithChildShapes(childShape.ChildShape, shapes);
                }
                return;
            }

            var scaledTriangleMeshShape = shape as ScaledBvhTriangleMeshShape;
            if (scaledTriangleMeshShape != null)
            {
                GetShapeWithChildShapes(scaledTriangleMeshShape.ChildShape, shapes);
                return;
            }

            var uniformScalingShape = shape as UniformScalingShape;
            if (uniformScalingShape != null)
            {
                GetShapeWithChildShapes(uniformScalingShape.ChildShape, shapes);
                return;
            }
        }
        #endregion
    }
}
